#include "bsp_distance.h"

char *distance_char[] = {"  F_DISTANCE: ", "  B_DISTANCE: ", "  L_DISTANCE: ", "  R_DISTANCE: "};
int distance_io[] = {F_DISTANCE, B_DISTANCE, L_DISTANCE, R_DISTANCE};
uint8_t mode_distance[4] = {0};

int data_f_distance[20] = {0};
int data_b_distance[20] = {0};
int data_l_distance[20] = {0};
int data_r_distance[20] = {0};
int *data_addr[] = {data_f_distance, data_b_distance, data_l_distance, data_r_distance};

/**
 * @brief 初始化红外测距模块
 * @param void
 * @return void
 * */
void distance_Init()
{
  for (int i = 0; i < 4; i++)
  {
    pinMode(distance_io[i], INPUT);
  }
}
//有bug待修复
///**
// * @brief 打印所有红外测距模块测量值
// * @param void
// * @return void
// * */
//void printDistance()
//{
//    int analogVal = 0;
//    for (int i = 0; i < 4; i++)
//    {
//        Serial.print(distance_char[i]);
//        analogVal = analogRead(distance_io[i]);
//        delay(10);
//        Serial.print(calculateVal(analogVal,i));      
//    }
//    Serial.println();
//}

/**
 * @brief 获取红外测距模块的测量值
 * @param uint8_t choice
 * @return void
 * */
void getDistance(int *pMem, uint8_t choice)
{
  uint8_t current = 0;

  choice_distance(choice);
  
  for (int i = 0; i < 4; i++)
  {
    if (mode_distance[i])
    {
      pMem[current] = calculateVal(distance_io[i]);

//                  Serial.print(current);
//                  Serial.print(" ");
//                 Serial.println(pMem[current]);
//                 Serial.println(calculateVal(distance_io[i]));
      current++;
    }
  }
}

/**
 * @brief 选择要操作的红外测距模块
 * @param uint8_t choice
 * @return void
 * */
void choice_distance(uint8_t choice)
{
  for (int i = 0; i < 4; i++)
  {
    mode_distance[i] = (choice & (1 << i)) >> i;
  }
}

/**
 * @brief 计算模拟值对应的距离(中位值平均滤波)
 * @param int analogVal
 * @return int distance
 * */
int calculateVal(int analogIO)
{
  
  int distance = 0;
  long sum = 0;
  int temp[SAMPLING_NUM] = {0};
  int change;
  int i, j;
  int *data = data_addr[analogIO];
  
  for(i = 0; i < SAMPLING_NUM; i++)
  {
//    temp[i] = 2548 / (analogRead(analogIO) / 2 - 10);
  temp[i] = analogRead(analogIO);
  }
  //冒泡法排序
  for (i = 0; i < SAMPLING_NUM -1; i++)
  {
    for (j = 0; j < SAMPLING_NUM - i - 1; j++)
    {
      if (temp[i] > temp[i + 1])
      {
        change = temp[i];
        temp[i] = temp[i + 1];
        temp[i + 1] = change;
      }
    }
  }
  for (i = MEDIAN_AVERAGE_REMOVE_NUM; i <SAMPLING_NUM - MEDIAN_AVERAGE_REMOVE_NUM; i++)
  {
    sum += temp[i];
  }

  return sum / (2 * MEDIAN_AVERAGE_REMOVE_NUM);
}

// int calculateVal_aver(int analogIO)
// {
//   int sum = 0;
//   int temp[20] = {0};
//   int distance;
//   int *data = data_addr[analogIO];
//   int i;

//   for (int i = 0; i < 20; i++)
//   {
//     data[i] = 2548 / (analogRead(analogIO) / 2 - 10);
//     Serial.println(data[i]);
//   }

//   for (i = 0; i < 20; i++)
//   {
//     sum += data[i];
//   }

//   return sum / 20;
// }
